#include "Mailboxes.h"

MAILBOX mailboxArray[mailboxSize];
TASK  waitedTasks [waitedSize];

void initializeWaitedTasks(void)
{
  char i;
  i=waitedSize-1;   
  do
  {
    waitedTasks[i].u8activation=ACTIVATE;
    waitedTasks[i].u8autostart=FALSE;
    waitedTasks[i].u8contex=NOCONTEXTED;
    waitedTasks[i].u8priority=0;
    waitedTasks[i].u8schedule=FULL;
    waitedTasks[i].u8state=READY;
  }while(i--);
}

void initializeMailboxArray(void)
{
  char i;
  i=mailboxSize-1;
  do
  {
    mailboxArray[i].dato=0;
    mailboxArray[i].u8bandera=FALSE;
  }while(i--);
}

void tomaMailbox(unsigned char mailboxID)
{
  index=waitedSize-1; 
  if(mailboxArray[mailboxID].u8bandera)
  {
    mailboxArray[mailboxID].u8bandera=FALSE;
    do
    {
      if((waitedTasks[index].u8mailboxID==mailboxID) && 
      (waitedTasks[index].u8state==WAITING))
      {
        /* Activa bandera */
        mailboxArray[mailboxID].u8bandera=TRUE;
        /* Meto dato */
        mailboxArray[mailboxID].dato=waitedTasks[index].u8mailboxData;
        waitedTasks[index].u8state=READY;
        insertTask(&waitedTasks[index]);
        taskPrioritize();
        break;
      }  
    }while(index--); 
  }
  else
  {
    asm
    {
      TSX
      STHX goBackSP
      /* Save the ccr, acumulator, h and x registers */
      /* H-X register */ 
      STHX spSAddH 
      /* Get H */ 
      LDHX #hAdd                 
      MOV  spSAddH,X+
      /* Get X */   
      LDHX #xAdd                             
      MOV  spSAddL,X+
      /* Get Accumulator */ 
      STA  spSAddH
      LDHX #aAdd
      MOV  spSAddH,X+
      /* Get CCR */
      TPA
      STA  spSAddH
      LDHX #ccrAdd
      MOV  spSAddH,X+ 
      /* Save the stack pointer */
      LDHX 3,SP       /* Load in HX register the sp value from the stak */ 
      STHX spSAddH    /* Save the HX register value in  spStak */
      LDHX #spAddH    /* Load to HX the direction of the first byte where */
                      /* I will save the firt bytes PC value */
      MOV  spSAddH,X+ /* Save the first byte PC in the direction 0x181 */
      LDHX #spAddL    /* Load to HX the direction of the second byte where */
                      /* I will save the firt bytes PC value */
      MOV  spSAddL,X+ /* Save the second byte PC in the direction 0x182 */
    }
    goBackSP=goBackSP+2;
    fptr->u8activation=NOACTIVATE; 
    fptr->u8state=WAITING;
    fptr->u8contex=CONTEXTED;
    /* Salvar el contexto en la estructura*/
    fptr->u16stackPointer=sp;
    fptr->u8aRegister=aReg;
    fptr->u8hRegister=hReg;
    fptr->u8xRegister=xReg;
    fptr->u8ccrRegister=ccrReg;
    fptr->u16goBackSP=goBackSP;
    sendWaitedTask();
    asm
    {
      PULX
      PULX
      PULX
      PULX
      PULX
      PULX
      LDHX #loopFor 
      JMP ,X
    }     
  }
}

void activaMailbox(unsigned char mailboxID, unsigned char dato)
{
  index=waitedSize-1;
  if(mailboxArray[mailboxID].u8bandera)
  {
    /* Send to wait */
    fptr->u8mailboxData=dato;
    asm
    {
      TSX
      STHX goBackSP
      /* Save the ccr, acumulator, h and x registers */
      /* H-X register */ 
      STHX spSAddH 
      /* Get H */ 
      LDHX #hAdd                 
      MOV  spSAddH,X+
      /* Get X */   
      LDHX #xAdd                             
      MOV  spSAddL,X+
      /* Get Accumulator */ 
      STA  spSAddH
      LDHX #aAdd
      MOV  spSAddH,X+
      /* Get CCR */
      TPA
      STA  spSAddH
      LDHX #ccrAdd
      MOV  spSAddH,X+ 
      /* Save the stack pointer */
      LDHX 3,SP       /* Load in HX register the sp value from the stak */ 
      STHX spSAddH    /* Save the HX register value in  spStak */
      LDHX #spAddH    /* Load to HX the direction of the first byte where */
                      /* I will save the firt bytes PC value */
      MOV  spSAddH,X+ /* Save the first byte PC in the direction 0x181 */
      LDHX #spAddL    /* Load to HX the direction of the second byte where */
                      /* I will save the firt bytes PC value */
      MOV  spSAddL,X+ /* Save the second byte PC in the direction 0x182 */
    } 
    goBackSP=goBackSP+4;
    fptr->u8activation=NOACTIVATE; 
    fptr->u8state=WAITING;//waited
    fptr->u8contex=CONTEXTED;
    //salvar el contexto en la estructura
    fptr->u16stackPointer=sp;
    fptr->u8aRegister=aReg;
    fptr->u8hRegister=hReg;
    fptr->u8xRegister=xReg;
    fptr->u8ccrRegister=ccrReg;
    fptr->u16goBackSP=goBackSP;
    sendWaitedTask();
  }
  else
  {
    /* Activa bandera */
    mailboxArray[mailboxID].u8bandera=TRUE;
    /* Meto dato */
    mailboxArray[mailboxID].dato=dato;
    asm
    {
      TSX
      STHX goBackSP
      /* Save the ccr, acumulator, h and x registers */
      /* H-X register */ 
      STHX spSAddH 
      /* Get H */ 
      LDHX #hAdd                 
      MOV  spSAddH,X+
      /* Get X */   
      LDHX #xAdd                             
      MOV  spSAddL,X+
      /* Get Accumulator */ 
      STA  spSAddH
      LDHX #aAdd
      MOV  spSAddH,X+
      /* Get CCR */
      TPA
      STA  spSAddH
      LDHX #ccrAdd
      MOV  spSAddH,X+ 
      /* Save the stack pointer */
      LDHX 3,SP       /* Load in HX register the sp value from the stak */ 
      STHX spSAddH    /* Save the HX register value in  spStak */
      LDHX #spAddH    /* Load to HX the direction of the first byte where */
                      /* I will save the firt bytes PC value */
      MOV  spSAddH,X+ /* Save the first byte PC in the direction 0x181 */
      LDHX #spAddL    /* Load to HX the direction of the second byte where */
                      /* I will save the firt bytes PC value */
      MOV  spSAddL,X+ /* Save the second byte PC in the direction 0x182 */
    }
    goBackSP=goBackSP+4;
    fptr->u8contex=CONTEXTED;
    /* Salvar el contexto en la estructura */
    fptr->u16stackPointer=sp;
    fptr->u8aRegister=aReg;
    fptr->u8hRegister=hReg;
    fptr->u8xRegister=xReg;
    fptr->u8ccrRegister=ccrReg;
    fptr->u16goBackSP=goBackSP;
    do
    {
      if((waitedTasks[index].u8mailboxID==mailboxID) && 
      (waitedTasks[index].u8state==WAITING))
      {
        waitedTasks[index].u8state=READY;
        /* Activate a waited task */
        waitedTasks[index].u8activation=NOACTIVATE; 
        waitedTasks[index].u8state=READY;
        insertTask(&waitedTasks[index]);
        taskPrioritize();
        break;
      }  
    }while(index--); 
  }
  asm
  {
    PULX
    PULX
    PULX
    PULX
    LDHX #loopFor 
    JMP ,X
  }          
}

void sendWaitedTask(void)
{
  unsigned char i=waitedSize-1;
  do
  {
    if(waitedTasks[i].u8state==READY)
    {
      waitedTasks[i]=ejectTask();
      return;
    }  
  }while(i--);
}